﻿using System;
using System.Collections.Generic;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Documents;
using Microsoft.Research.Kinect.Nui;
using Matrix = Microsoft.Xna.Framework.Matrix;
using Vector = Microsoft.Research.Kinect.Nui.Vector;
using Vector3 = Microsoft.Xna.Framework.Vector3;
using Kinect;
using Kinect.GPart;

namespace Kinect
{
    /// <summary>
    /// DepthViewer.xaml 的交互逻辑
    /// </summary>
    public partial class DepthViewer : UserControl
    {
        public bool getplane;
        public DepthViewer()
        {
            points = new List<Point>();
            LeftUpPoint = NullPoint;
            LeftDownPoint = NullPoint;
            RightDownPoint = NullPoint;
            RightUpPoint = NullPoint;
            InitializeComponent();
            getplane = false;
        }

        private Runtime _Kinect = Runtime.Kinects[0]; 
        public delegate void AlreadyGetPlaneEventHandler(object sender);
        public AlreadyGetPlaneEventHandler PlaneAlready;
        
        public void _Kinect_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            PlanarImage planarImage = e.ImageFrame.Image;
            depthBits = planarImage.Bits;
            convertedDepthBits = convertDepthFrame(depthBits, realDepth);

            int width = 320, height = 240;

            foreach (Point p in points)
            {
                int x = (int)Math.Round(p.X * width);
                int y = (int)Math.Round(p.Y * height);
                 GFuck.FuckPoint(convertedDepthBits, width, height, 5, x, y, 255, 0, 0);
            }
            do
            {
                int x = (int)Math.Round(LeftUpPoint.X * width);
                int y = (int)Math.Round(LeftUpPoint.Y * height);
                GFuck.FuckPoint(convertedDepthBits, width, height, 5, x, y, 0, 0, 255);
            } while (false);

            do
            {
                int x = (int)Math.Round(RightUpPoint.X * width);
                int y = (int)Math.Round(RightUpPoint.Y * height);
                GFuck.FuckPoint(convertedDepthBits, width, height, 5, x, y, 0, 0, 255);
            } while (false);

            do
            {
                int x = (int)Math.Round(LeftDownPoint.X * width);
                int y = (int)Math.Round(LeftDownPoint.Y * height);
                GFuck.FuckPoint(convertedDepthBits, width, height, 5, x, y, 0, 0, 255);
            } while (false);

            do
            {
                int x = (int)Math.Round(RightDownPoint.X * width);
                int y = (int)Math.Round(RightDownPoint.Y * height);
                GFuck.FuckPoint(convertedDepthBits, width, height, 5, x, y, 0, 0, 255);
            } while (false);

            if (depthImageHelper == null)
            {
                depthImageHelper = new InteropBitmapHelper(planarImage.Width, planarImage.Height, convertedDepthBits);
                kinectDepthImage.Source = depthImageHelper.InteropBitmap;
            }
            else
            {
                depthImageHelper.UpdateBits(convertedDepthBits);
            }
        }
        
        byte[] convertDepthFrame(byte[] depthFrame16, int[] realDepthFrame)
        {
            bool hasPlayerData = MyRuntimeOptions.HasFlag(RuntimeOptions.UseDepthAndPlayerIndex);
            int mindepth = -1, maxdepth = -1;
            for (int i16 = 0; i16 < depthFrame16.Length; i16 += 2)
            {
                int player = hasPlayerData ? depthFrame16[i16] & 0x07 : -1;
                int realDepth = 0;

                if (hasPlayerData)
                    realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);
                else
                    realDepth = (depthFrame16[i16 + 1] << 8) | (depthFrame16[i16]);

                if (realDepth == 0) continue;

                if (mindepth == -1 || realDepth < mindepth) mindepth = realDepth;
                if (maxdepth == -1 || realDepth > maxdepth) maxdepth = realDepth;
            }

            for (int i16 = 0, i32 = 0, i8 = 0; i16 < depthFrame16.Length && i32 < depthFrame32.Length; i16 += 2, i32 += 4, i8++)
            {
                int realDepth = 0;

                if (hasPlayerData)
                    realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);
                else
                    realDepth = (depthFrame16[i16 + 1] << 8) | (depthFrame16[i16]);

                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)

//                byte intensity = (byte)(255 - (255 * realDepth / 0x0fff));
                byte intensity = (byte)(255 * (realDepth - mindepth) / (double)(maxdepth - mindepth));

                realDepthFrame[i8] = realDepth;
                depthFrame32[i32 + GParam.RedIndex] = (byte)(intensity);
                depthFrame32[i32 + GParam.GreenIndex] = (byte)(intensity);
                depthFrame32[i32 + GParam.BlueIndex] = (byte)(intensity);
            }
            return depthFrame32;
        }
                

        private void btnCalcPlane_Click(object sender, RoutedEventArgs e)
        {
            List<Vector3> points3d = new List<Vector3>();
            List<Vector3> points2d = new List<Vector3>();
            int width = 320, height = 240;
            int[] realDepthBak = new int[320 * 240];
            Array.Copy(realDepth, realDepthBak, 320 * 240);
            GPoint[] poly = { new GPoint(LeftDownPoint), new GPoint(RightDownPoint), new GPoint(RightUpPoint), new GPoint(LeftUpPoint) };
            List<GPoint> gpoints = new List<GPoint>();
            for (int i = 0; i < GParam.depthWidth; i++)
            {
                for (int j = 0; j < GParam.depthHeight; j++)
                {
                    GPoint pt = new GPoint(i / GParam.depthWidthf, j / GParam.depthHeightf);
                    if (pt.inPoly(poly))
                    {
                        gpoints.Add(pt);
                    }
                }
            }

            double avgZ = 0;

            //for (int i = 0; i < 30; i++)
            //{
            //    ImageFrame imageFrame = _Kinect.DepthStream.GetNextFrame(0);
            //    if (imageFrame == null) continue;
            //    foreach (GPoint pt in gpoints)
            //    {
            //        int depthX = (int)Math.Round(pt.x * width);
            //        int depthY = (int)Math.Round(pt.y * height);
            //        int n = (depthX + depthY * width) * 2;
            //        int depthValue = imageFrame.Image.Bits[n] + (imageFrame.Image.Bits[n+1] << 8);
            //        Vector vec = _Kinect.SkeletonEngine.DepthImageToSkeleton(depthX / (float)width, depthY / (float)height, (short)(depthValue));
            //        Vector3 vec3 = new Vector3(vec.X, vec.Y, vec.Z);
            //        points3d.Add(vec3);
            //        avgZ += vec3.Z;
            //    }
            //}



            foreach (GPoint pt in gpoints)
            {
                int depthX = (int)Math.Round(pt.x * width);
                int depthY = (int)Math.Round(pt.y * height);
                int n = depthX + depthY * width;
                int depthValue = realDepthBak[n] << 3;
                Vector vec = _Kinect.SkeletonEngine.DepthImageToSkeleton(depthX / (float)width, depthY / (float)height, (short)(depthValue));
                Vector3 vec3 = new Vector3(vec.X, vec.Y, vec.Z);
                points3d.Add(vec3);
                points2d.Add( new Vector3(depthX / (float)width, depthY / (float)height, depthValue ));
                avgZ += vec3.Z;
            }
            Vector3[] poly3 = new Vector3[4];
            for (int i = 0; i < 4; i++)
            {
                int depthX = (int)Math.Round(poly[i].x * width);
                int depthY = (int)Math.Round(poly[i].y * height);
                int n = depthX + depthY * width;
                int depthValue = realDepthBak[n] << 3;
                Vector vec = _Kinect.SkeletonEngine.DepthImageToSkeleton(depthX / (float)width, depthY / (float)height, (short)(depthValue));
                Vector3 vec3 = new Vector3(vec.X, vec.Y, vec.Z);
                poly3[i] = vec3;
            }
            avgZ /= points3d.Count;
            labelTmp.Content = avgZ;//points3d.Count;

            Vector plane = G3D.getPlane(points3d);
            double tt = 0;
            foreach (Vector3 vec3d in points3d)
            {
                tt = Math.Max(tt, Math.Abs(vec3d.X * plane.X + vec3d.Y * plane.Y + vec3d.Z * plane.Z + plane.W));
                tt /= Math.Sqrt(plane.X * plane.X + plane.Y * plane.Y + plane.Z * plane.Z);
            }
//            tt = G3D.getVariance(points3d, plane);

            textPlane.Text = string.Format("z = \n{0}x\n{1}y\n{2}", plane.X
                , plane.Y, plane.W);


            labelTmp.Content = tt;//points3d.Count;


            gFileWriter.open();
            gFileWriter.writeln(plane.X);
            gFileWriter.writeln(plane.Y);
            gFileWriter.writeln(plane.W);

            for (int i = 0; i < 4; i++)
            {
                gFileWriter.writeln(poly3[i]);
            }

            points3d.Clear();
            foreach (GPoint pt in gpoints)
            {
                int depthX = (int)Math.Round(pt.x * width);
                int depthY = (int)Math.Round(pt.y * height);
                int n = depthX + depthY * width;
                int depthValue = realDepthBak[n] << 3;
                Vector3 vec3 = new Vector3(depthX / (float)width, depthY / (float)height, depthValue);
                points3d.Add(vec3);
            }
            plane = G3D.getPlane(points2d);

            gFileWriter.writeln(plane.X);
            gFileWriter.writeln(plane.Y);
            gFileWriter.writeln(plane.W);
            gFileWriter.close();
            if (this.PlaneAlready != null)
            {
                this.PlaneAlready(e);
            }
        }
        
        private void kinectDepthImage_MouseLeftButtonDown(object sender, System.Windows.Input.MouseButtonEventArgs e)
        {
            System.Windows.Point Position;
            Position = e.GetPosition(kinectDepthImage);
            DepthXPersentage = Position.X / GParam.depthFrameWidthf;
            DepthYPersentage = Position.Y / GParam.depthFrameHeightf;

            PointDepthPersentage.Text = string.Format("{0:0.00}% x {1:0.00}%", DepthXPersentage * 100, DepthYPersentage * 100);

            points.Add(new Point(DepthXPersentage, DepthYPersentage));
            LeftUpPoint = LeftDownPoint = RightUpPoint = RightDownPoint = points[0];
            foreach (Point pt in points)
            {
                if (pt.X + pt.Y < LeftUpPoint.X + LeftUpPoint.Y)
                    LeftUpPoint = pt;
                if (pt.X + pt.Y > RightDownPoint.X + RightDownPoint.Y)
                    RightDownPoint = pt;
                if (pt.X - pt.Y > RightUpPoint.X - RightUpPoint.Y)
                    RightUpPoint = pt;
                if (pt.X - pt.Y < LeftDownPoint.X - LeftDownPoint.Y)
                    LeftDownPoint = pt;
            }
            Point[] criticalPoints = { LeftUpPoint, RightUpPoint, LeftDownPoint, RightDownPoint };
            for (int k = 0; k < criticalPoints.Length; k++)
            {
                double avgX = 0, avgY = 0;
                int cnt = 0;
                foreach (Point pt in points)
                {
                    if (GMath.hypot(pt.X - criticalPoints[k].X, pt.Y - criticalPoints[k].Y) < 0.05)
                    {
                        avgX += pt.X;
                        avgY += pt.Y;
                        ++cnt;
                    }
                }
                avgX /= cnt;
                avgY /= cnt;
                criticalPoints[k] = new Point(avgX, avgY);
            }
            LeftUpPoint = criticalPoints[0];
            RightUpPoint = criticalPoints[1];
            LeftDownPoint = criticalPoints[2];
            RightDownPoint = criticalPoints[3];

            labelTmp.Content = points.Count.ToString();
        }

        private List<Point> points;
        private Point LeftUpPoint, LeftDownPoint, RightUpPoint, RightDownPoint, NullPoint = new Point(-1, -1);

        byte[] depthBits;
        byte[] convertedDepthBits;
        private byte[] depthFrame32 = new byte[640 * 480 * 4];
        private int[] realDepth = new int[640 * 480 * 4];
//        private int frameCalc = 0;

        private double DepthXPersentage = -1, DepthYPersentage = -1;

        public RuntimeOptions MyRuntimeOptions;//remember!!!!

        private GFileWriter gFileWriter = new GFileWriter();

        private InteropBitmapHelper depthImageHelper = null;
    }

}
